#ifndef _MATRIX4_H_
#define _MATRIX4_H_

#include "Vector3.h"
#include "Vector4.h"
#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

#include <cmath>
#include <cstdlib>
#include <iostream>
#include <cassert>
#include "../basecode/BasicMath.h"

using namespace std;

class Matrix4
{
 public:
	double m[4][4];   // matrix elements
	static Matrix4 identityMatrix;

	bool isEqualTo(Matrix4 mat) {
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				if (abs(m[i][j]-mat.get(i,j)) > 0.000000000000001) {
					return false;
				}
			}
		}
		return true;
	}
	
	Matrix4(double val=0)
		{
			for (int i=0; i<4; ++i)
				{
					for (int j=0; j<4; ++j)
						{
							m[i][j] = val;
						}
				}
		} 
	
	Matrix4(
		double m00, double m01, double m02, double m03,
		double m10, double m11, double m12, double m13,
		double m20, double m21, double m22, double m23,
		double m30, double m31, double m32, double m33 )
		{
			m[0][0] = m00;
			m[0][1] = m01;
			m[0][2] = m02;
			m[0][3] = m03;
			m[1][0] = m10;
			m[1][1] = m11;
			m[1][2] = m12;
			m[1][3] = m13;
			m[2][0] = m20;
			m[2][1] = m21;
			m[2][2] = m22;
			m[2][3] = m23;
			m[3][0] = m30;
			m[3][1] = m31;
			m[3][2] = m32;
			m[3][3] = m33;
		}
	
	void set(
		 double m00, double m01, double m02, double m03,
		 double m10, double m11, double m12, double m13,
		 double m20, double m21, double m22, double m23,
		 double m30, double m31, double m32, double m33 )
	{
		m[0][0] = m00;
		m[0][1] = m01;
		m[0][2] = m02;
		m[0][3] = m03;
		m[1][0] = m10;
		m[1][1] = m11;
		m[1][2] = m12;
		m[1][3] = m13;
		m[2][0] = m20;
		m[2][1] = m21;
		m[2][2] = m22;
		m[2][3] = m23;
		m[3][0] = m30;
		m[3][1] = m31;
		m[3][2] = m32;
		m[3][3] = m33;
	}
	
	double get(int i, int j) {
		return m[i][j];
	}
	
	double* getPointer()
	{
		return &m[0][0];
	}

	Matrix4 transpose() {
		Matrix4 result;
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				result.m[i][j] = m[j][i];
			}
		}
		return result;
	}

	GLdouble* getGLMatrix() 
	{
		GLdouble *transpose = new GLdouble[16];
		
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
                                transpose[4*i+j] = m[j][i];
			}
		}
		
		return transpose;
	}
	
	void identity()
	{
		double ident[4][4]={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
		for (int i=0; i<4; ++i)
			{
				for (int j=0; j<4; ++j)
					{
						m[i][j] = ident[i][j];
					}
			}
	}

	Vector3 getTranslation() {
		return Vector3(m[0][3], m[1][3], m[2][3]);
	}
	
	// angle in degrees
	void rotateX(double angle)
	{
		double th = BasicMath::radian(angle);
		m[0][0] = 1;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		
		m[1][0] = 0;
		m[1][1] = cos(th);
		m[1][2] = -sin(th);
		m[1][3] = 0;
		
		m[2][0] = 0;
		m[2][1] = sin(th);
		m[2][2] = cos(th);
		m[2][3] = 0;
		
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}
	
	// angle in degrees
	void rotateY_degrees(double angle)
	{
		double th = BasicMath::radian(angle);
		m[0][0] = cos(th);
		m[0][1] = 0;
		m[0][2] = sin(th);
		m[0][3] = 0;
		
		m[1][0] = 0;
		m[1][1] = 1;
		m[1][2] = 0;
		m[1][3] = 0;
		
		m[2][0] = -sin(th);
		m[2][1] = 0;
		m[2][2] = cos(th);
		m[2][3] = 0;
		
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}
	
	// angle in degrees
	void rotateZ(double angle)
	{
		double th = BasicMath::radian(angle);
		m[0][0] = cos(th);
		m[0][1] = -sin(th);
		m[0][2] = 0;
		m[0][3] = 0;
		
		
		m[1][0] = sin(th);
		m[1][1] = cos(th);
		m[1][2] = 0;
		m[1][3] = 0;
		
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = 1;
		m[2][3] = 0;
		
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}
	
	Matrix4 operator*(const Matrix4 &mat) {
		return (*this).multiply(mat);
	}

	Vector4 operator*(const Vector4 &vec) {
		return (*this).multiply(vec);
	}

	Vector3 operator*(const Vector3 &vec) {
		return (*this).multiply(vec);
	}
	
	Matrix4 multiply(const Matrix4 &mat)
	{
		Matrix4 result;
		
		for (int i=0; i<4; i++) { // for each row 
			for (int j=0; j<4; j++) { // for each column
				// sum the products of the individual elements
				
				double sum=0;
				for (int k=0; k<4; k++) {
					sum += m[i][k] * mat.m[k][j];
				}
				
				
				result.m[i][j] = sum;
			}
		}
		
		return result;
	}
	
	Vector4 multiply(const Vector4 &vec)
	{
		Vector4 result;
		
		for (int i=0; i<4; i++) { // for row 
			// sum the products of the individual elements
			
			double sum=0;
			for (int k=0; k<4; k++) {
				sum += m[i][k] * vec[k];
			}
			
			
			result.v[i] = sum;
		}
		
		return result;
	}
	
	Vector3 multiply(const Vector3 &vec)
	{
		Vector4 v(vec.v[0],vec.v[1],vec.v[2],1);
		Vector4 v2 = multiply(v);
		v2.dehomogenize();
		return Vector3(v2.v[0], v2.v[1], v2.v[2]);
	}
	
	void print() {
		cout << "[" << endl;
		for (int i=0; i<4; i++) {
			cout << " [ ";
			for (int j=0; j<4; j++) {
				cout << m[i][j] << " ";
			}
			cout << " ] ";
			cout << endl;
		}
		cout << "]" << endl;
	}
	
	static Matrix4 createRotationMatrix(Vector3 axis, double th)
	{
		Matrix4 result;
		result.identity();

		axis.normalize();
		
		double X=axis[0];
		double Y=axis[1];
		double Z=axis[2];
		
		double COS = cos(th);
		double SIN = sin(th);
		
		// First row
		result.m[0][0] = 1+(1-COS)*(X*X-1);
		result.m[0][1] = -Z*SIN+(1-COS)*X*Y;
		result.m[0][2] = Y*SIN+(1-COS)*X*Z;
		
		// Second row
		result.m[1][0] = Z*SIN+(1-COS)*Y*X;
		result.m[1][1] = 1+(1-COS)*(Y*Y-1);
		result.m[1][2] = -X*SIN+(1-COS)*Y*Z;
		
		// Third row
		result.m[2][0] = -Y*SIN+(1-COS)*Z*X;
		result.m[2][1] = X*SIN+(1-COS)*Z*Y;
		result.m[2][2] = 1+(1-COS)*(Z-1);
		
		return result;
	}
	
	static Matrix4 createScaleMatrix(double v1, double v2, double v3)
	{
		Matrix4 res; res.identity();
		res.m[0][0] = v1;
		res.m[1][1] = v2;
		res.m[2][2] = v3;
		res.m[4][4] = 1;
		
		return res;
	}
	
	static Matrix4 createTranslationMatrix(Vector3 v)
	{
		double v1 = v[0];
		double v2 = v[1];
		double v3 = v[2];

		Matrix4 res(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
		res.m[0][0] = 1;
		res.m[1][1] = 1;
		res.m[2][2] = 1;
		res.m[3][3] = 1;
		
		res.m[0][3] = v1;
		res.m[1][3] = v2;
		res.m[2][3] = v3;

		return res;
	}

	void turnIntoTranslationMatrix(double v1, double v2, double v3)
	{
		m[0][0] = 1;
		m[1][1] = 1;
		m[2][2] = 1;
		m[3][3] = 1;
		
		m[0][3] = v1;
		m[1][3] = v2;
		m[2][3] = v3;
	}
	
	Matrix4 getInverse() {
                double inv[16], mat[16], det;
		
		for (int i=0; i<4; i++) {
			mat[i] = m[i][0];
		}
		for (int i=0; i<4; i++) {
			mat[i+4] = m[i][1];
		}
		for (int i=0; i<4; i++) {
			mat[i+8] = m[i][2];
		}
		for (int i=0; i<4; i++) {
			mat[i+12] = m[i][3];
		}
		/*
		cout << "MAT IS NOW" << endl;
		for (int i=0; i<16; i++) {
			cout << mat[i] << " ";
		}
		cout << endl;
		*/

                inv[0] =   mat[5]*mat[10]*mat[15] - mat[5]*mat[11]*mat[14] - mat[9]*mat[6]*mat[15]
                        + mat[9]*mat[7]*mat[14] + mat[13]*mat[6]*mat[11] - mat[13]*mat[7]*mat[10];
                inv[4] =  -mat[4]*mat[10]*mat[15] + mat[4]*mat[11]*mat[14] + mat[8]*mat[6]*mat[15]
                        - mat[8]*mat[7]*mat[14] - mat[12]*mat[6]*mat[11] + mat[12]*mat[7]*mat[10];
                inv[8] =   mat[4]*mat[9]*mat[15] - mat[4]*mat[11]*mat[13] - mat[8]*mat[5]*mat[15]
                        + mat[8]*mat[7]*mat[13] + mat[12]*mat[5]*mat[11] - mat[12]*mat[7]*mat[9];
                inv[12] = -mat[4]*mat[9]*mat[14] + mat[4]*mat[10]*mat[13] + mat[8]*mat[5]*mat[14]
                        - mat[8]*mat[6]*mat[13] - mat[12]*mat[5]*mat[10] + mat[12]*mat[6]*mat[9];
                inv[1] =  -mat[1]*mat[10]*mat[15] + mat[1]*mat[11]*mat[14] + mat[9]*mat[2]*mat[15]
                        - mat[9]*mat[3]*mat[14] - mat[13]*mat[2]*mat[11] + mat[13]*mat[3]*mat[10];
                inv[5] =   mat[0]*mat[10]*mat[15] - mat[0]*mat[11]*mat[14] - mat[8]*mat[2]*mat[15]
                        + mat[8]*mat[3]*mat[14] + mat[12]*mat[2]*mat[11] - mat[12]*mat[3]*mat[10];
                inv[9] =  -mat[0]*mat[9]*mat[15] + mat[0]*mat[11]*mat[13] + mat[8]*mat[1]*mat[15]
                        - mat[8]*mat[3]*mat[13] - mat[12]*mat[1]*mat[11] + mat[12]*mat[3]*mat[9];
                inv[13] =  mat[0]*mat[9]*mat[14] - mat[0]*mat[10]*mat[13] - mat[8]*mat[1]*mat[14]
                        + mat[8]*mat[2]*mat[13] + mat[12]*mat[1]*mat[10] - mat[12]*mat[2]*mat[9];
                inv[2] =   mat[1]*mat[6]*mat[15] - mat[1]*mat[7]*mat[14] - mat[5]*mat[2]*mat[15]
                        + mat[5]*mat[3]*mat[14] + mat[13]*mat[2]*mat[7] - mat[13]*mat[3]*mat[6];
                inv[6] =  -mat[0]*mat[6]*mat[15] + mat[0]*mat[7]*mat[14] + mat[4]*mat[2]*mat[15]
                        - mat[4]*mat[3]*mat[14] - mat[12]*mat[2]*mat[7] + mat[12]*mat[3]*mat[6];
                inv[10] =  mat[0]*mat[5]*mat[15] - mat[0]*mat[7]*mat[13] - mat[4]*mat[1]*mat[15]
                        + mat[4]*mat[3]*mat[13] + mat[12]*mat[1]*mat[7] - mat[12]*mat[3]*mat[5];
                inv[14] = -mat[0]*mat[5]*mat[14] + mat[0]*mat[6]*mat[13] + mat[4]*mat[1]*mat[14]
                        - mat[4]*mat[2]*mat[13] - mat[12]*mat[1]*mat[6] + mat[12]*mat[2]*mat[5];
                inv[3] =  -mat[1]*mat[6]*mat[11] + mat[1]*mat[7]*mat[10] + mat[5]*mat[2]*mat[11]
                        - mat[5]*mat[3]*mat[10] - mat[9]*mat[2]*mat[7] + mat[9]*mat[3]*mat[6];
                inv[7] =   mat[0]*mat[6]*mat[11] - mat[0]*mat[7]*mat[10] - mat[4]*mat[2]*mat[11]
                        + mat[4]*mat[3]*mat[10] + mat[8]*mat[2]*mat[7] - mat[8]*mat[3]*mat[6];
                inv[11] = -mat[0]*mat[5]*mat[11] + mat[0]*mat[7]*mat[9] + mat[4]*mat[1]*mat[11]
                        - mat[4]*mat[3]*mat[9] - mat[8]*mat[1]*mat[7] + mat[8]*mat[3]*mat[5];
                inv[15] =  mat[0]*mat[5]*mat[10] - mat[0]*mat[6]*mat[9] - mat[4]*mat[1]*mat[10]
                        + mat[4]*mat[2]*mat[9] + mat[8]*mat[1]*mat[6] - mat[8]*mat[2]*mat[5];

                det = mat[0]*inv[0] + mat[1]*inv[4] + mat[2]*inv[8] + mat[3]*inv[12];
                if (det == 0)
                        assert(false);

                det = 1.0 / det;
		/*	cout << "INV IS NOW" << endl;
		for (int i=0; i<16; i++) {
			cout << inv[i] << " ";
		}
		cout << endl;
		*/

                Matrix4 result; result.identity();
		for (int i=0; i<4; i++) {
			result.m[i][0] = inv[i];
                }

                for (int i=0; i<4; i++) {
			result.m[i][1] = inv[i+4];
                }

                for (int i=0; i<4; i++) {
			result.m[i][2] = inv[i+8];
		}

		for (int i=0; i<4; i++) {
			result.m[i][3] = inv[i+12];
                }

		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				result.m[i][j] *= det;
			}
		}
		return result;
        }


};

Matrix4 Matrix4::identityMatrix = Matrix4(1,0,0,0,
					  0,1,0,0,
					  0,0,1,0,
					  0,0,0,1);



#endif

